%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%参考文献【基于三次非均匀B样条曲线的路径平滑技术研究_范银辉】(33页)
% 注意这篇论文的直线速度规划章节中的减速度段的位移公式是错误的,修正方法可以参考加速度段的位移公式，减速度的a1应该是-a
% 位移公式: S = S_0 + V_0 * t + 1/2 * a * t^2
% 其中：
% S    - 位移 (单位: 米)
% S_0  - 初始位置 (单位: 米)
% V_0  - 初始速度 (单位: 米/秒)
% t    - 时间 (单位: 秒)
% a    - 加速度 (单位: 米/秒²)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 设：
% 初始位移 q_0
% 起始速度 V_0
% 末速度   V_1
% 加速时间 T_a
% 减速时间 T_d
% 均速时间 T_v
% 加速度   a
% 减速度   a_1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

V = 3000;           % 给定速度
a = 100;            % 加速度
a1 = -a;            % 减速度
T_uv = 50;          % 匀速运动的时间
q_0 = 0;            % 起始位移
V0 = 0;             % 起始速度

% 加速度段
T_acc = (V - V0) / a;                       % 给定加速度和速度的情况下达到目标速度的时间
T_dec = T_acc;                              % 加速度时间等于减速度时间
time_acc = 0 : 0.1 : T_acc;                 % 时间的颗粒度
S_acc = q_0 + V0 * time_acc + 1/2 * a * time_acc.^2;  % 加速段的位移
V_acc = V0 + a * time_acc;                        % 加速度段的速度
A_acc = ones( size(time_acc) ) * a;
% 均速度段
time_uv = 0 : 0.1 : T_uv;
S_uv = S_acc(end) + V * time_uv;
V_uv = ones( size(time_uv) ) * V;
A_uv = ones( size(time_uv) ) * 0;
% 减速度段
time_dec = 0 : 0.1 : T_dec;
S_dec = S_uv(end) + V_uv(end) * time_dec + 1/2 * a1 * time_dec.^2;  % 加速段的位移
V_dec = V_uv(end) + a1 * time_dec;
A_dec = ones( size(time_dec) ) * a1;

total_time = [time_acc, time_uv + T_acc, time_dec + T_acc + T_uv]; % 合并时间
% 合成总位移
total_displacement = [S_acc, S_uv, S_dec];  % 合并三段位移
total_velocity = [V_acc, V_uv, V_dec];
total_acceleration = [A_acc, A_uv, A_dec];
% 绘图
figure;
subplot( 2, 2, 1 );
plot(total_time, total_displacement, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位移 (mm)');      % y轴标签
title('总的位移');        % 图形标题
grid on;                  % 显示网格

subplot( 2, 2, 2 );
plot(total_time, total_acceleration, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位置 (mm)');      % y轴标签
title('总的加速度');        % 图形标题
grid on;                  % 显示网格

subplot( 2, 2, 3 );
plot(total_time, total_velocity, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位置 (mm)');      % y轴标签
title('总的速度');        % 图形标题
grid on;                  % 显示网格